//
// Created by sukai on 22-9-19.
//

#include <iostream>
#include <ros/ros.h>
#include <signal.h>
using namespace std;
/**
 *
 * @param argc
 * @param argv
 * @return
 * C++打印出当前运行的所有ROS节点
 * 获取ros节点 和topic
 * 2023-01-10
 * sukai
 * rosrun contnav test2_node
 */


//ctrl + c
void mySigintHandler(int sig)
{
    std::cout << "ctrl c  sig: "<<sig<<std::endl;

    ros::shutdown();
    if (ros::isShuttingDown()){
        cout<<"close ros  test.cpp"<<endl;
    }
}
//字符串分割函数 [callback_mapNameList 获取地图名使用该函数]
std::vector<std::string> split(std::string pattern,std::string str)
{
    try {

        std::string::size_type pos;
        std::vector<std::string> result;
        str+=pattern; //扩展字符串以方便操作
        int  size=str.size();

        for ( int  i=0; i<size; i++)
        {
            pos=str.find(pattern,i);
            if (pos<size)
            {
                std::string s=str.substr(i,pos-i);
                result.push_back(s);
                i=pos+pattern.size()-1;
            }
        }


        return  result;

    }catch (std::exception e){

    }

}

bool getNodes(std::vector<std::string> &nodes)
{
    //C++打印出当前运行的所有ROS节点
    ros::V_string v_nodes;
    bool fig =ros::master::getNodes(v_nodes);
    if(!fig){
        cout << "未获取到ROS节点 "  << endl;
        return false;
    }
    for (std::string elem : v_nodes) {
        cout << "C++打印出当前运行的所有ROS节点: " <<elem << endl;
        //按/号截取
        std::vector<std::string> list = split("/",elem);
        cout << "listnode.size() " <<list.size() << endl;
        cout << "listnode[0]  " <<list[1] << endl;
        if(list.size()>1)
          nodes.push_back(list[1]);

    }

//    C++打印出当前运行的所有ROS节点: /rosout
//    C++打印出当前运行的所有ROS节点: /test2_node
    return true;
}
/**
 *
 * result_topicNames     把 / 去掉的数据  /map 变 map
 * result_topicdatatype  把 / 替换:: 的数据,datatype: nav_msgs/OccupancyGrid这个数据需要把/替换成::后填入代码中;datatype: nav_msgs::OccupancyGrid
 *
 * result_topicNameOriginaldatas 原数据
 * result_topicdatatypeOriginaldatas 原数据
 */
bool getTopics(std::vector<std::string> &result_topicNames)
{
    //C++打印出当前运行的所有ROSTopic
    std::vector<ros::master::TopicInfo> V_TopicInfos;
    bool fig =ros::master::getTopics(V_TopicInfos);
    if(!fig){
        cout << "未获取到 topic "  << endl;
        return false;
    }
    for (ros::master::TopicInfo topicInfo: V_TopicInfos) {
        cout << "C++打印出当前运行的所有topic name: " <<topicInfo.name << endl;
        cout << "C++打印出当前运行的所有topic datatype: " <<topicInfo.datatype << endl;

        //按/号截取
        std::vector<std::string> list = split("/",topicInfo.name);
        cout << "listtopic.size() " <<list.size() << endl;
        cout << "listtopic[0]  " <<list[1] << endl;
        if(list.size()>1)
            result_topicNames.push_back(list[1]);

    }

//    C++打印出当前运行的所有topic name: /rosout_agg
//    C++打印出当前运行的所有topic datatype: rosgraph_msgs/Log
//    C++打印出当前运行的所有topic name: /rosout
//    C++打印出当前运行的所有topic datatype: rosgraph_msgs/Log
    return true;
}
// rosrun contnav test2_node
int main(int argc,char *argv[]){
    //节点名
    string nodeName = "test2_contnav_contnav_global_planner_node";
    //初始化节点
    ros::init(argc,argv,nodeName);
    //创建节点
    ros::NodeHandle node;
    //监控 ctrl c
    signal(SIGINT, mySigintHandler);
    std::vector<std::string> nodes;
    std::vector<std::string> topics;
    getNodes(nodes);//C++打印出当前运行的所有ROS节点
    getTopics(topics);//C++打印出当前运行的所有ROSTopic

    string host= ros::master::getHost();//返回 master所处的hostname
    int port=  ros::master::getPort();//返回master运行在的端口
    string uri=  ros::master::getURI();//返回到master的URL地址，如http://host:port/
    bool checkfig=  ros::master::check();//检查master是否启动

    cout << "C++打印出当前master所处的hostname: " <<host << endl;
    cout << "C++打印出当前master运行在的端口: " <<port << endl;
    cout << "C++打印出当前master的URL地址: " <<uri << endl;
    cout << "C++打印出当前检查master是否启动: " <<checkfig<< endl;

//    C++打印出当前master所处的hostname: localhost
//    C++打印出当前master运行在的端口: 11311
//    C++打印出当前master的URL地址: http://localhost:11311
//    C++打印出当前检查master是否启动: 1


    //ros入门教程
    //https://blog.csdn.net/Chen_2018k/article/details/104334367
    //事件轮询
    ros::spin();
    return 0;
}


